/*
 * Copyright (c) 2022, www.lgmgim.cn
 * All rights reserved.
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *  * Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *  * Neither the name of the University of California, Berkeley nor the
 *    names of its contributors may be used to endorse or promote products
 *    derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/*
 * @file pallet_controller.cpp
 * @author gc
 * @date May 29, 2023
 * @brief pallet operation
 * Contact: congcong.guan@lgmgim.cn
 */
#include <agv_peripheral_controller/toolKit.h>
#include <agv_peripheral_controller/amr/pallet_controller.h>
#include <transform/transform.h>

namespace agv_peripheral {


double getRemainder(double x, double y)
{
    int n = trunc(x/y);
    return x - n*y;
}

double getErr(double x, double y) 
{
    int n = round(x/y);
    return x - n*y;
}


double normalizationRad(double rad)
{
    double tmp = getRemainder(rad, 2 * M_PI);
    if (tmp > M_PI)
        return tmp - 2 * M_PI;
    if (tmp < -1 * M_PI)
        return tmp + 2 * M_PI;

    return tmp;
}

PalletController::PalletController(std::shared_ptr<rclcpp::Node> n):node(n)
{
    bool init_result = init();
    assert(init_result);
}


bool PalletController::init()
{
    rotary_pub = node->create_publisher<std_msgs::msg::Float64MultiArray>(
      "/rotary_controller/commands", 10);
    
    jacking_pub = node->create_publisher<std_msgs::msg::Float64MultiArray>(
      "/jacking_controller/commands", 10);

    odometry_sub = node->create_subscription<agv_msgs::msg::OdometryLite>(
      "/odom", 10,
      std::bind(&PalletController::odometrycallback, this, std::placeholders::_1));

    cmd_vel_sub = node->create_subscription<geometry_msgs::msg::TwistStamped>(
      "/cmd_vel", 10,
      std::bind(&PalletController::cmdvelcallback, this, std::placeholders::_1));

    joint_sub = node->create_subscription<sensor_msgs::msg::JointState>(
      "/base/joint_states", 10,
      std::bind(&PalletController::jointStateCallback, this, std::placeholders::_1));

    trigger_service = node->create_service<agv_srvs::srv::FollowTrigger>(
      "follow_trigger",
      std::bind(&PalletController::follow_triggercallback, this,
                std::placeholders::_1, std::placeholders::_2));

    node->get_parameter_or<double>("JackingMaxPos", jacking_max_position, 0.05);
    node->get_parameter_or<double>("JackingMinPos", jacking_min_position, 0.00);
    node->get_parameter_or<int>("RotaryResetScale", reset_scale, 0);
    node->get_parameter_or<double>("RotaryFollowMultiplier", rotary_multiplier, 1.0);
    node->get_parameter_or<double>("RotaryAngularControlKP", rotary_angular_control_kp, 2.0);
    node->get_parameter_or<double>("RotaryAngularControlKI", rotary_angular_control_ki, 0.05);
    node->get_parameter_or<double>("RotaryAngularControlKD", rotary_angular_control_kd, 0.2);
    node->get_parameter_or<double>("RotaryAngularControlCoeff", feedforward_comp_coeff, 0.8);
    node->get_parameter_or<double>("RotaryAngularControlAcc", rotary_angular_control_acc, 0.8);

    node->get_parameter_or<double>("RotaryFixSpeed", rotary_fix_speed, 0.1);
    node->get_parameter_or<double>("RotaryResetSpeed", rotary_reset_speed, 0.8);
    node->get_parameter_or<double>("RotaryTaskSpeed", rotary_task_speed, 0.8);
    double fix_threshold;
    node->get_parameter_or<double>("RotaryFixThreshold", fix_threshold, 0.1);
    rotary_fix_threshold = abs(fix_threshold / 180.0 * M_PI);

    DEBUG_OUT("[PalletController] init  get rotary p[%.2f] i[%.2f] d[%.2f] c[%.2f] fix_threshold: %.4f",  
        rotary_angular_control_kp, rotary_angular_control_ki, rotary_angular_control_kd, feedforward_comp_coeff, rotary_fix_threshold);
    DEBUG_OUT("[PalletController] reset scale: %d", reset_scale);
    
    rotary_angular_speed_target.data = { 0.0 };
    jacking_position_target.data = { 0.0 };
    return true;
}


bool PalletController::follow_triggercallback(const std::shared_ptr<agv_srvs::srv::FollowTrigger::Request> request,
    std::shared_ptr<agv_srvs::srv::FollowTrigger::Response> response)
{
    response->status = true;
    set_rotary_target_position(request->target / 180.0 * M_PI * -1);
    DEBUG_OUT("[PalletController] test triggered");
    return true;
}


void PalletController::odometrycallback(const agv_msgs::msg::OdometryLite::ConstSharedPtr& msg)
{
    odom_pose = msg->pose;
    using namespace transform;
    Transform tf = Transform::Identity();
    tf.setTransform(odom_pose);
    
    current_chassis_yaw = tf.GetYaw();
    current_chassis_linear_speed = msg->twist.linear.x;
    current_chassis_angular_speed = msg->twist.angular.z;

    pallet_yaw = getPalletYaw(current_chassis_yaw, rotary_position);
}


void PalletController::cmdvelcallback(const geometry_msgs::msg::TwistStamped& cmd)
{
    cmd_vel_update_time = node->now();
    current_chassis_angular_speed_target = cmd.twist.angular.z;
}


void PalletController::jointStateCallback(const sensor_msgs::msg::JointState::ConstSharedPtr &msg)
{
    joint_state = *msg;

    // std::thread::id pid = std::this_thread::get_id();
    // DEBUG_OUT("pid: %d  seq: %d  pos1: %.8f  pos2: %.8f", pid, joint_state.header.seq, joint_state.position[0], joint_state.position[1]);

    if (!find_joint_id)
    {
        bool find_jacking = false;
        bool find_rotary = false;
        for (int i = 0; i < joint_state.name.size(); i++) {
            if (!find_jacking && joint_state.name[i].find(JACKING_JOINT) != std::string::npos) {
                jacking_id = i;
                find_jacking = true;
                DEBUG_OUT("[PalletController] JACKING ID:%d", jacking_id);
            }
            if (!find_rotary && joint_state.name[i].find(ROTARY_JOINT) != std::string::npos) {
                rotary_id = i;
                find_rotary = true;
                DEBUG_OUT("[PalletController] ROTARY ID:%d", rotary_id);
            }
        }

        if (find_jacking && find_rotary)
            find_joint_id = true;
        else
            RCLCPP_ERROR(rclcpp::get_logger("AgvPeripheralController"), "[PalletController] Could not find peripheral jonit number");
    }
    else if (find_joint_id)
    {
        jacking_position = joint_state.position[jacking_id];
        rotary_position = joint_state.position[rotary_id];
        current_pallet_angular_speed = joint_state.velocity[rotary_id];
    }
            
    if ((jacking_position - jacking_min_position) > 0.005)
        jacking_up_flag = true;
    else
        jacking_up_flag = false;
}


double PalletController::getPalletYaw(double chassis_yaw, double rotary_position)
{
    double pallet_rad = normalizationRad(rotary_position);
    return normalizationRad(chassis_yaw - pallet_rad);
}

double PalletController::getRadDistance(double target, double current, bool absolute)
{
    if (absolute)
        return current - target;
    else
        return normalizationRad(current - target);
}

int PalletController::set_rotary_target_position(double target)
{
    if (state == IDLE) {
        int n = round(getRadDistance(target, rotary_position) / (2 * M_PI));
        target_pallet_position = normalizationRad(target + n * 2 * M_PI);
        DEBUG_OUT("[PalletController] TASK  get target: %.6f set target: %.6f current_position: %.6f",
            target, target_pallet_position, rotary_position);
        
        if (abs(getRadDistance(target_pallet_position,rotary_position)) < 0.005) {
            DEBUG_OUT("[PalletController] TASK already at target position");
            return 1;
        }

        acc_start_time = time_now;
        state = TASK;
        return 0;
    } else {
        DEBUG_OUT("[PalletController] set rotary target failed. controller busy [%d]", state);
        return 1;
    }
}

int PalletController::set_rotary_target_velocity(double target)
{
    rotary_angular_speed_target.data[0] = target;
    if (jacking_up_flag) {
        double temp = 1.0;
        node->get_parameter_or<double>("RotaryFollowMultiplier", temp, 1.0);
        if (abs(temp - 1.0) < 1.0 && abs(temp - rotary_multiplier) > 0.00001) {
            rotary_multiplier = temp;
            DEBUG_OUT("[PALLET_CONTROLLER] set new rotary_multiplier: %.4f", rotary_multiplier);
        }
        
        rotary_angular_speed_target.data[0] *= rotary_multiplier;
    }
    
    rotary_pub->publish(rotary_angular_speed_target);
    return 0;
}


int PalletController::set_jacking_target_position(double target)
{
    jacking_target_updated = true;
    jacking_position_target.data[0] = target;
    // jacking_pub->publish(jacking_position_target);
    return 0;
}

void PalletController::execute_jacking_operation()
{
    if (state == IDLE && jacking_target_updated && !rotary_fix_enable) {
        jacking_pub->publish(jacking_position_target);
        jacking_target_updated = false;
    }
    else {
        if (jacking_target_updated) {  
            DEBUG_OUT_THROTTLE(node->get_clock(), 1000, "[PALLET_CONTROLLER] jacking blocked, state[%d] fix_enable[%d]", state, rotary_fix_enable);
        }
    }
}

void PalletController::update()
{   
    DEBUG_OUT_THROTTLE(node->get_clock(), 1000, "[PALLET_CONTROLLER] current state: %d", state);
    time_now = node->now();
    pallet_yaw = getPalletYaw(current_chassis_yaw, rotary_position);

    last_state = state;
    double reset_err = 0.0;
    double follow_err = 0.0;

    switch (state) {
        case IDLE:
            if (follow_onoff && jacking_up_flag) {
                target_yaw = pallet_yaw;
                state = FOLLOW;
            }

            if (!jacking_up_flag && jacking_up_flag != last_jacking_up_flag && !manual_mode) {
                if (rotary_forbid == 0) {
                    state = RESET;   
                }
                else {
                    DEBUG_OUT("[PalletController] rotary forbid[%d] !!!", rotary_forbid);
                }
            }

            if (rotary_forbid == 0 && rotary_forbid != last_rotary_forbid && !jacking_up_flag && !manual_mode) {
                state = RESET;
            }

            if (jacking_target_updated && abs(jacking_position_target.data[0] - jacking_min_position) < 0.001
                && abs(jacking_position - jacking_max_position) < 0.001 && !manual_mode) {
                state = FIX;
                rotary_fix_enable = true;
            }
            break;

        case FOLLOW:
            if (!follow_onoff) {
                state = FIX;
            }
            else
                rotaryFollow();
            break;

        case FIX:
            if (rotary_fix_enable && abs(getRadDistance(target_yaw,pallet_yaw)) > 0.001) {
                DEBUG_OUT_THROTTLE(node->get_clock(), 100, "[PalletController] Fix  target_pallet_position: %.6f  current_position: %.6f", 
                    target_pallet_position, rotary_position);
                rotaryControl(target_yaw, pallet_yaw, state);
            }
            else {
                state = IDLE;
                rotary_fix_enable = false;
            }
            break;

        case RESET:
            if (reset_scale == 0) {
                reset_err = abs(getRadDistance(target_pallet_position,rotary_position, true));
            }
            else {
                reset_err = abs(getRadDistance(target_pallet_position,rotary_position));
            }
            
            if (reset_err > 0.001) {
                DEBUG_OUT_THROTTLE(node->get_clock(), 100, "[PalletController] RESET  target_pallet_position: %.6f  current_position: %.6f", 
                    target_pallet_position, rotary_position);
                rotaryControl(target_pallet_position, rotary_position, state);
            }
            else
                state = IDLE;
            break;

        case TASK:
            // TODO: rotate to nearest specific angle exclude accumulated cycles
            if (abs(getRadDistance(target_pallet_position,rotary_position)) > 0.001)
                rotaryControl(target_pallet_position, rotary_position, state);
            else
                state = IDLE;
            break;

        default:
            DEBUG_OUT("[PALLET_CONTROLLER] unknown state [%d] set IDLE", state);
            state = IDLE;
    }

    int n = 0;
    if (state != last_state) {
        switch (state) {
            case IDLE:
                DEBUG_OUT("[PALLET_CONTROLLER] state switch to IDLE");
                rotary_angular_speed_target.data[0] = 0.0;
                rotary_pub->publish(rotary_angular_speed_target); 
                break;

            case FOLLOW:
                DEBUG_OUT("[PALLET_CONTROLLER] state switch to FOLLOW");
                break;

            case FIX:
                DEBUG_OUT("[PALLET_CONTROLLER] state switch to FIX");
                n = round(getRadDistance(current_chassis_yaw, pallet_yaw) / (M_PI/2));
                target_yaw = normalizationRad(current_chassis_yaw + n * M_PI / 2);

                follow_err = abs(getRadDistance(target_yaw, pallet_yaw));
                DEBUG_OUT("[PalletController] follow err: %.4f  fix_threshold: %.4f", follow_err, rotary_fix_threshold);
                if (follow_err > rotary_fix_threshold) {
                    rotary_fix_enable = true;
                }
                
                break;

            case RESET:
                acc_start_time = time_now;
                DEBUG_OUT("[PALLET_CONTROLLER] state switch to RESET");
                    
                if (reset_scale == 0) {
                    // reset to absolute zero
                    target_pallet_position = 0.0;
                }
                else if (reset_scale == 2) {
                    // reset to nearest direction
                    n = round(getRadDistance(0.0, rotary_position) / (2 * M_PI));
                    target_pallet_position = normalizationRad(0.0 + n * 2 * M_PI);
                }
                break;

            case TASK:
                acc_start_time = time_now;
                DEBUG_OUT("[PALLET_CONTROLLER] state switch to TASK");
                break;

            default:
                DEBUG_OUT("[PALLET_CONTROLLER] unknow state[%d], set IDLE", state);
        }
    }

    last_rotary_forbid = rotary_forbid;
    last_jacking_up_flag = jacking_up_flag;
    execute_jacking_operation();
}

void PalletController::rotaryFollow()
{
    rotary_angular_speed_target.data[0] = current_chassis_angular_speed_target;
    double temp = 1.0;
    node->get_parameter_or<double>("RotaryFollowMultiplier", temp, 1.0);
    if (abs(temp - 1.0) < 1.0 && abs(temp - rotary_multiplier) > 0.00001) {
        rotary_multiplier = temp;
        DEBUG_OUT("[PALLET_CONTROLLER] set new rotary_multiplier: %.4f", rotary_multiplier);
    }
 
    rotary_angular_speed_target.data[0]  *= rotary_multiplier;
    rotary_pub->publish(rotary_angular_speed_target);
}


void PalletController::rotaryControl(double target, double current, int current_state)
{
    pallet_yaw_err[4] = pallet_yaw_err[3];
    pallet_yaw_err[3] = pallet_yaw_err[2];
    pallet_yaw_err[2] = pallet_yaw_err[1];
    pallet_yaw_err[1] = pallet_yaw_err[0];
    if (current_state == RESET) {
        if (reset_scale == 0) {
            pallet_yaw_err[0] = getRadDistance(target, current, true);
        }
        else {
            pallet_yaw_err[0] = getRadDistance(target, current);
        }
    }
    else
        pallet_yaw_err[0] = getRadDistance(target, current);
    
    pallet_yaw_integral = 0.0;
    for (int i=0; i<5; i++) {
        pallet_yaw_integral += pallet_yaw_err[i];
    }

    rotary_angular_speed_target.data[0] = pallet_yaw_err[0] * rotary_angular_control_kp +
        pallet_yaw_integral * rotary_angular_control_ki +
        (pallet_yaw_err[0] - pallet_yaw_err[1]) * rotary_angular_control_kd;

    if (current_state == TASK || current_state == RESET) {
       
        rotary_angular_speed_target.data[0]  *= -1.0;
        
        double speed_limit = (time_now - acc_start_time).seconds() * rotary_angular_control_acc;
        rotary_angular_speed_target.data[0] = rotary_angular_speed_target.data[0]> speed_limit ? speed_limit : rotary_angular_speed_target.data[0];
        rotary_angular_speed_target.data[0] = rotary_angular_speed_target.data[0]< (speed_limit * -1) ? (speed_limit * -1) : rotary_angular_speed_target.data[0]; 
        if (current_state == TASK) {
            rotary_angular_speed_target.data[0]= rotary_angular_speed_target.data[0]> rotary_task_speed ? rotary_task_speed : rotary_angular_speed_target.data[0];
            rotary_angular_speed_target.data[0]= rotary_angular_speed_target.data[0]< (-1 * rotary_task_speed) ? (-1 * rotary_task_speed) : rotary_angular_speed_target.data[0];
        } else if (current_state == RESET) {
            rotary_angular_speed_target.data[0]= rotary_angular_speed_target.data[0]> rotary_reset_speed ? rotary_reset_speed : rotary_angular_speed_target.data[0];
            rotary_angular_speed_target.data[0]= rotary_angular_speed_target.data[0]< (-1 * rotary_reset_speed) ? (-1 * rotary_reset_speed) : rotary_angular_speed_target.data[0];
        }

    }
    else if (current_state == FIX) {
        rotary_angular_speed_target.data[0]= rotary_angular_speed_target.data[0]> rotary_fix_speed ? rotary_fix_speed : rotary_angular_speed_target.data[0];
        rotary_angular_speed_target.data[0]= rotary_angular_speed_target.data[0]< (-1 * rotary_fix_speed) ? (-1 * rotary_fix_speed) : rotary_angular_speed_target.data[0];
    }


    // DEBUG_OUT("[PalletFollow][RESET]  t_position: %.4f p_position: %.4f err: %.4f p_speed: %.4f pt_speed: %.4f END", 
    //    target_pallet_position, rotary_position, pallet_yaw_err[0], current_pallet_angular_speed, rotary_angular_speed_target.data);
    rotary_pub->publish(rotary_angular_speed_target);
}

}; // end namespace
